{
  "cells": [
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "# NonlinearEquality\n",
        "\n",
        "The `NonlinearEquality` family of factors in GTSAM provides constraints to enforce equality between variables or between a variable and a constant value. These factors are useful in optimization problems where strict equality constraints are required. Below is an overview of the API, grouped by functionality."
      ]
    },
    {
      "cell_type": "markdown",
      "id": "colab_button",
      "metadata": {},
      "source": [
        "<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/nonlinear/doc/NonlinearEquality.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "license_cell",
      "metadata": {
        "tags": [
          "remove-cell"
        ]
      },
      "source": [
        "GTSAM Copyright 2010-2022, Georgia Tech Research Corporation,\nAtlanta, Georgia 30332-0415\nAll Rights Reserved\n\nAuthors: Frank Dellaert, et al. (see THANKS for the full author list)\n\nSee LICENSE for the license information"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "id": "colab_import",
      "metadata": {
        "tags": [
          "remove-cell"
        ]
      },
      "outputs": [],
      "source": [
        "try:\n    import google.colab\n    %pip install --quiet gtsam-develop\nexcept ImportError:\n    pass"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## NonlinearEquality\n",
        "\n",
        "The `NonlinearEquality` factor enforces equality between a variable and a feasible value. It supports both exact and inexact evaluation modes.\n",
        "\n",
        "### Constructors\n",
        "- `NonlinearEquality(Key j, const T& feasible, const CompareFunction& compare)`  \n",
        "  Creates a factor that enforces exact equality between the variable at key `j` and the feasible value `feasible`.  \n",
        "  - `j`: Key of the variable to constrain.  \n",
        "  - `feasible`: The feasible value to enforce equality with.  \n",
        "  - `compare`: Optional comparison function (default uses `traits<T>::Equals`).\n",
        "\n",
        "- `NonlinearEquality(Key j, const T& feasible, double error_gain, const CompareFunction& compare)`  \n",
        "  Creates a factor that allows inexact evaluation with a specified error gain.  \n",
        "  - `error_gain`: Gain applied to the error when the constraint is violated.\n",
        "\n",
        "### Methods\n",
        "- `double error(const Values& c) const`  \n",
        "  Computes the error for the given values. Returns `0.0` if the constraint is satisfied, or a scaled error if `allow_error_` is enabled.\n",
        "\n",
        "- `Vector evaluateError(const T& xj, OptionalMatrixType H = nullptr) const`  \n",
        "  Evaluates the error vector for the given variable value `xj`. Optionally computes the Jacobian matrix `H`.\n",
        "\n",
        "- `GaussianFactor::shared_ptr linearize(const Values& x) const`  \n",
        "  Linearizes the factor at the given values `x` to create a Gaussian factor."
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## NonlinearEquality1\n",
        "\n",
        "The `NonlinearEquality1` factor is a unary equality constraint that fixes a variable to a specific value.\n",
        "\n",
        "### Constructors\n",
        "- `NonlinearEquality1(const X& value, Key key, double mu = 1000.0)`  \n",
        "  Creates a factor that fixes the variable at `key` to the value `value`.  \n",
        "  - `value`: The fixed value for the variable.  \n",
        "  - `key`: Key of the variable to constrain.  \n",
        "  - `mu`: Strength of the constraint (default: `1000.0`).\n",
        "\n",
        "### Methods\n",
        "- `Vector evaluateError(const X& x1, OptionalMatrixType H = nullptr) const`  \n",
        "  Evaluates the error vector for the given variable value `x1`. Optionally computes the Jacobian matrix `H`.\n",
        "\n",
        "- `void print(const std::string& s, const KeyFormatter& keyFormatter) const`  \n",
        "  Prints the factor details, including the fixed value and noise model."
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## NonlinearEquality2\n",
        "\n",
        "The `NonlinearEquality2` factor is a binary equality constraint that enforces equality between two variables.\n",
        "\n",
        "### Constructors\n",
        "- `NonlinearEquality2(Key key1, Key key2, double mu = 1e4)`  \n",
        "  Creates a factor that enforces equality between the variables at `key1` and `key2`.  \n",
        "  - `key1`: Key of the first variable.  \n",
        "  - `key2`: Key of the second variable.  \n",
        "  - `mu`: Strength of the constraint (default: `1e4`).\n",
        "\n",
        "### Methods\n",
        "- `Vector evaluateError(const T& x1, const T& x2, OptionalMatrixType H1 = nullptr, OptionalMatrixType H2 = nullptr) const`  \n",
        "  Evaluates the error vector for the given variable values `x1` and `x2`. Optionally computes the Jacobian matrices `H1` and `H2`.\n",
        "\n",
        "- `void print(const std::string& s, const KeyFormatter& keyFormatter) const`  \n",
        "  Prints the factor details, including the keys and noise model."
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## Common Features\n",
        "\n",
        "### Error Handling Modes\n",
        "- Exact Evaluation: Throws an error during linearization if the constraint is violated.  \n",
        "- Inexact Evaluation: Allows nonzero error and scales it using the `error_gain_` parameter.\n",
        "\n",
        "### Serialization\n",
        "All factors support serialization for saving and loading models.\n",
        "\n",
        "### Testable Interface\n",
        "All factors implement the `Testable` interface, providing methods like:\n",
        "- `void print(const std::string& s, const KeyFormatter& keyFormatter) const`  \n",
        "  Prints the factor details.\n",
        "- `bool equals(const NonlinearFactor& f, double tol) const`  \n",
        "  Checks if two factors are equal within a specified tolerance."
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "These factors provide a flexible way to enforce equality constraints in nonlinear optimization problems, making them useful for applications like SLAM, robotics, and control systems."
      ]
    }
  ],
  "metadata": {
    "language_info": {
      "name": "python"
    }
  },
  "nbformat": 4,
  "nbformat_minor": 2
}